#pragma once
// #autor:yxj
#include "rclcpp/rclcpp.hpp"
#include <agv_peripheral_controller/toolKit.h>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
// 急停
#define EMG_BUTTON "emergencyButton"
// 液压举升
#define UP_LIMIT_FRONT    "upLimitFront"
#define DOWN_LIMIT_FRONT  "downLimitFront"
#define UP_LIMIT_REAR     "upLimitRear"
#define DOWN_LIMIT_REAR   "downLimitRear"

#define CONTROL_IO_FRONT    "frontControl"
#define CONTROL_IO_REAR     "rearControl"


// 开关索引
enum Switch
{
    ioLiftUp,
    ioLiftDown,
    normalLiftUp,
    normalLiftDown
};

// 开关类型
enum class SwitchType
{
    ioLiftType,
    normalLiftType
};


#define SET_ONE_BIT(x) (((uint64_t)1) << (x))
#define TEST_BIT(x, i) ((x & SET_ONE_BIT(i)) != 0)
#define SET_N_BIT(x, n) (x | ((uint64_t)(1) << n))

// 手自动模式获取比特位(agv_info)
#define MANUL_BIT 4
#define HALF_BIT 22
#define AUTO_ONLINE_BIT 23

namespace two_diff_agv_peripheral
{
    // 定义输入IO点类型
    using INPUT_IO_TYPE = CDataTimeStamp<agv_msgs::msg::PairTypeInt>;

    // 定义输出控制IO点类型
    // 参数： first:io名字 second:控制输出的电平
    using OUTPUT_IO_TYPE = agv_msgs::msg::PairTypeInt;

    // IO控制输出组
    using IO_GROUP = std::vector<OUTPUT_IO_TYPE>;

    // 检测输入IO信号是否触发
    // 返回：是否触发
    inline bool inputIo(INPUT_IO_TYPE &inputIo)
    {

        bool ret = false;
        // 如果数据有效，判断是否触发
        bool timeOut = inputIo.TimeOut(300);
        bool pressed = inputIo.Get().value;

        if (timeOut)
        {
            DEBUG_ERROR_OUT("inputIo:data time out:key=" << inputIo.Get().key);
            return ret;
        }

        ret = pressed;

        return ret;
    }

    // 单举升类
    class CIoLift
    {
    public:
        // 默认构造
        CIoLift();

        // 默认析构
        ~CIoLift();

        // 初始化
        bool init(ros::ServiceClient &client);

        // 举升上升
        bool up();

        // 举升下降
        bool down();

        // 举升停止
        bool stop();

        // 货物装填
        bool loadStatus();

        // 前上限位
        INPUT_IO_TYPE upLimit;

        // 前下限位
        INPUT_IO_TYPE downLimit;

        // 控制IO点
        IO_GROUP ctrl;

    private:
        // 上升到位
        bool upTarget();

        // 下降到位
        bool downTarget();

        // 控制IO点输出 数组：param1:索引  param2:触发
        // 返回：成功与否
        bool outputIo(IO_GROUP outIoGroup);

        // 最后输出io的电平状态
        IO_GROUP m_outIoRecord;

        // 控制IO输出客户端
        ros::ServiceClient m_client;
    };

    // IO举升控制类
    class CIoLifterController
    {
    public:
        // 默认构造函数
        CIoLifterController();

        // 析构函数
        ~CIoLifterController();

        // 是否初始化完成
        bool initFinish();

        // 初始化函数
        bool init();

        // 举升上升
        bool up();

        // 举升下降
        bool down();

        // 举升停止
        bool stop();

        // 货物装填
        bool loadStatus();

        // 急停是否按下
        bool emgPressed();

    private:
        // 获取新IO输入信号回调函数
        void readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew &msg);

        // 更新输入IO对象
        void updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj);

    private:
        // 前举升
        CIoLift m_frontLift;

        // 后举升
        CIoLift m_rearLift;

        // 初始化完成标识
        bool m_init;

        // 私有节点句柄
        rclcpp::Node m_priNh;

        // 话题发布者
        // ros::Publisher m_pub;

        // 订阅IO输入信号话题
        ros::Subscriber m_subIo;

        // 订阅新的IO输入信号话题
        ros::Subscriber m_subIoNew;

        // 控制IO输出客户端
        ros::ServiceClient m_outClient;

        // 急停信号触发
        INPUT_IO_TYPE m_emgBtn;
    };
}